﻿using System;
using System.Collections.Generic;
using System.Text;
using FIRADriverLibrary;
using GeometryCSharp;

namespace AGHFiraPlayer.Strategies.StrategiesImpl
{
    public class BasicOffensiveStrategy : AbstractStrategy
    {
        // values used by all methods; for debug as well
        public Point2D kickingPoint;
        public double robotBallDistance;
        public double eta;

        public BasicOffensiveStrategy(ref AGHDriver driver)
        {
            this.driver = driver;
            type = StrategyType.OffnsiveHelp;
            involvedRobots = new List<RobotController>(1);
        }

        override public int applicabilityLevel(ref List<IStrategy> choosenStrategies)
        {
            //TODO: implement this method more "properly"
            foreach (RobotController robot in driver.Robots) {
                if (!robot.isUsedInStrategy)
                {
                    involvedRobots.Add(robot);
                    return 1;
                }
            }
            return -1;
        }


        override public void execute()
        {
            //if (driver.firaDriver.ballX > AGHDriver.pitchArea.Width / 2)
            //{
            //    PotencialDriver.DriveVelocity(involvedRobots[0].robotState, AGHDriver.pitchArea.Width - AGHDriver.ROBOT_DIAMETER, driver.firaDriver.ballY, 0.5);
            //}
            //else
            //{
            //    PotencialDriver.DriveVelocity(involvedRobots[0].robotState, AGHDriver.pitchArea.Width / 2, driver.firaDriver.ballY, PotencialDriver.tinyVelocity);
            //}
        }

        /*      public void execute(ref FIRADriver firaDriver, ref IList<RobotController> defenders, ref IList<RobotController> attackers, ref IList<FIRARobotState> oponents)
             {
                double minDefenderDistance = defenders[0].Position.GetDistance(firaDriver.ballX, firaDriver.ballY);
                 double distance;

                 for (int i = 1; i < defenders.Count; i++)
                 {
                     distance = defenders[i].Position.GetDistance(firaDriver.ballX, firaDriver.ballY);
                     if (distance < minDefenderDistance) 
                     {
                         minDefenderDistance = distance;
                     }
                 }

                 double eta = minDefenderDistance / PotencialDriver.maxVelocity;
                 Point2D ballAtETA = Predicator.GetBallPosition(eta);
                 RobotController kicker = defenders[0];

                 minDefenderDistance = ballAtETA.GetDistance(defenders[0].robotState.x, defenders[0].robotState.y);
                 for (int i = 1; i < defenders.Count; i++)
                 {
                     distance = ballAtETA.GetDistance(defenders[i].robotState.x, defenders[i].robotState.y);
                     if (distance < minDefenderDistance)
                     {
                         minDefenderDistance = distance;
                         kicker = defenders[i];
                     }
                 }

                 MoveToKickingPositionAndKick(kicker);
                 for (int i = 0; i < defenders.Count; i++)
                 {
                     if (defenders[i] == kicker)
                         continue;

                     distance = defenders[i].Position.GetDistance(defenders[i].startupPosition);
                     if (distance > GlobalVariables.ROBOT_DIAMETER)
                         PotencialDriver.Drive(defenders[i], defenders[i].startupPosition.x, defenders[i].startupPosition.y, distance / PotencialDriver.tinyVelocity);
                     else
                         defenders[i].Stop();
                 }
            
                 return;
  
             }
      */
        private void MoveToKickingPositionAndKick(RobotController robotController)
		{
			if (IsClearKickingPosition(robotController))
			{
				Kick(robotController);
				return;
			}
		}

		private bool IsClearKickingPosition(RobotController robotController)
		{
			return true;
		}

		private void Kick(RobotController robotController)
		{

		}



        /*     
        public void execute()
            {
               bool kicker = false;
                foreach (RobotController attacker in attackers)
                {
                    if (!kicker)
                    {
                        if (IsClearKick(attacker, ref firaDriver, ref oponents))
                        {
                            Kick(attacker, ref firaDriver);
                            kicker = true;
                        }
                        else
                        {
                            DefensivePosition(attacker, ref firaDriver);
                        }
                    }
                    else
                    {
                        SupportPosition(attacker, ref firaDriver);
                    }
                }
    
            }
         *  */
        /*
                /// <summary>
                /// 
                /// </summary>
                /// <param name="robotController"></param>
                /// <returns>estimated time to a kick</returns>
                private double FillKickingPoint(RobotController robotController, ref FIRADriver firaDriver)
                {
                    Vector2D ballRobotVector = new Vector2D(robotController.robotState.x - firaDriver.ballX, robotController.robotState.y - firaDriver.ballY);
                    double angleBallRobot = Vector2D.AngleBetween(Predicator.V, ballRobotVector);

                    double real_eta = ballRobotVector.Length / (PotencialDriver.maxVelocity + Predicator.V.Length * Math.Cos(angleBallRobot));
                    if (real_eta <= 0 || real_eta > GlobalVariables.pitchArea.Width / PotencialDriver.maxVelocity)
                        real_eta = GlobalVariables.pitchArea.Width / PotencialDriver.maxVelocity;
                    real_eta *= 1 + 0.1 * Math.Abs(Vector2D.AngleBetween(robotController.VelocityVersor, Predicator.V));
                    kickingPoint = Predicator.GetBallPosition(real_eta);
                    robotBallDistance = kickingPoint.GetDistance(robotController.robotState.x, robotController.robotState.y);
                    eta = robotBallDistance / PotencialDriver.maxVelocity;
                    return real_eta;

                    //			robotBallDistance = robotController.Position.GetDistance(firaDriver.ballX, firaDriver.ballY);
                    //			eta = robotBallDistance / PotencialDriver.maxVelocity;
                    //			kickingPoint = Predicator.GetBallPosition(eta);
                    //			robotBallDistance = (robotBallDistance + kickingPoint.GetDistance(robotController.robotState.x, robotController.robotState.y) ) / 2;
                    //			eta = robotBallDistance / PotencialDriver.maxVelocity;
                    //			kickingPoint = Predicator.GetBallPosition(eta);
                    //			return kickingPoint.GetDistance(robotController.robotState.x, robotController.robotState.y);
                }

                private bool IsClearKick(RobotController robotController, ref FIRADriver firaDriver, ref IList<FIRARobotState> oponents)
                {
                    //estimate position
                    FillKickingPoint(robotController, ref firaDriver);

                    //check alignment
                    if (robotController.robotState.x > kickingPoint.x)
                        return false;

                    //check distance
                    if (robotBallDistance > GlobalVariables.ROBOT_DIAMETER * 3)
                        return false;

                    //check obstacles - oponents only - if cooper is closer, he will be asked to kick
                    double minX = Math.Min(robotController.robotState.x, kickingPoint.x);
                    double minY = Math.Min(robotController.robotState.y, kickingPoint.y);
                    double maxX = Math.Max(robotController.robotState.x, kickingPoint.x);
                    double maxY = Math.Max(robotController.robotState.y, kickingPoint.y);
                    foreach (FIRARobotState obstacleRobot in oponents)
                    {
                        if (obstacleRobot.x > minX && obstacleRobot.x < maxX && obstacleRobot.y > minY && obstacleRobot.y < maxY)
                        {
                            return false;
                        }
                    }
                    return true;
                }

                private void Kick(RobotController robotController, ref FIRADriver firaDriver)
                {
                    FillKickingPoint(robotController, ref firaDriver);

                    Vector2D ballTargetVector = new Vector2D(GlobalVariables.target.x - kickingPoint.x, GlobalVariables.target.y - kickingPoint.y);
                    Vector2D robotBallVector = new Vector2D(kickingPoint.x - robotController.robotState.x, kickingPoint.y - robotController.robotState.y);

                    robotBallVector.Rotate(Vector2D.AngleBetween(ballTargetVector, robotBallVector) * 0.7);

                    kickingPoint.x = robotController.robotState.x + robotBallVector.X;
                    kickingPoint.y = robotController.robotState.y + robotBallVector.Y;

                    if (kickingPoint.x < GlobalVariables.pitchArea.Left)
                        kickingPoint.x = GlobalVariables.pitchArea.Left + 0.03;
                    if (kickingPoint.x > GlobalVariables.pitchArea.Right)
                        kickingPoint.x = GlobalVariables.pitchArea.Right - 0.03;
                    if (kickingPoint.y < GlobalVariables.pitchArea.Top)
                        kickingPoint.y = GlobalVariables.pitchArea.Top + 0.03;
                    if (kickingPoint.y > GlobalVariables.pitchArea.Bottom)
                        kickingPoint.y = GlobalVariables.pitchArea.Bottom - 0.03;

                    PotencialDriver.Drive(robotController, kickingPoint.x, kickingPoint.y, eta);

                    //super kick
                    //off
                    //			if (robotBallVector.Length < robotDiameter)
                    //			{
                    //				if (robotController.robotState.leftSpeed > robotController.robotState.rightSpeed)
                    //				{
                    //					robotController.robotState.rightSpeed /= 2;
                    //					robotController.robotState.leftSpeed *= 2;
                    //				}
                    //				else
                    //				{
                    //					robotController.robotState.leftSpeed /= 2;
                    //					robotController.robotState.rightSpeed *= 2;
                    //				}
                    //			}
                }

                private void KickingPosition(RobotController robotController, ref FIRADriver firaDriver)
                {
                    FillKickingPoint(robotController, ref firaDriver);

                    if ((robotController.robotState.y > kickingPoint.y || kickingPoint.y - 2 * GlobalVariables.ROBOT_DIAMETER < GlobalVariables.pitchArea.Top) && kickingPoint.y + 2 * GlobalVariables.ROBOT_DIAMETER < GlobalVariables.pitchArea.Bottom)
                        kickingPoint.y += 2 * GlobalVariables.ROBOT_DIAMETER;
                    else
                        kickingPoint.y -= 2 * GlobalVariables.ROBOT_DIAMETER;

                    kickingPoint.x -= 2 * GlobalVariables.ROBOT_DIAMETER;
                    if (kickingPoint.x < GlobalVariables.pitchArea.Left)
                        kickingPoint.x = GlobalVariables.pitchArea.Left + 0.03;

                    eta = kickingPoint.GetDistance(robotController.robotState.x, robotController.robotState.y) / PotencialDriver.maxVelocity;

                    PotencialDriver.Drive(robotController, kickingPoint.x, kickingPoint.y, eta);
                }

                private void DefensivePosition(RobotController robotController, ref FIRADriver firaDriver)
                {
                    //			robotController.Stop();
                    //			return;

                    kickingPoint.x = firaDriver.ballX;
                    kickingPoint.y = firaDriver.ballY;

                    kickingPoint.x = kickingPoint.x / 2;
                    kickingPoint.y = (kickingPoint.y + GlobalVariables.target.y) / 2;

                    eta = kickingPoint.GetDistance(robotController.robotState.x, robotController.robotState.y) / PotencialDriver.maxVelocity;

                    PotencialDriver.Drive(robotController, kickingPoint.x, kickingPoint.y, eta);
                }

                private void SupportPosition(RobotController robotController, ref FIRADriver firaDriver)
                {
                    //			robotController.Stop();
                    //			return;

                    kickingPoint.x = firaDriver.ballX;
                    kickingPoint.y = firaDriver.ballY;

                    if (kickingPoint.y > GlobalVariables.target.y)
                        kickingPoint.y /= 2;
                    else
                        kickingPoint.y += (GlobalVariables.pitchArea.Height - kickingPoint.y) / 2;

                    eta = kickingPoint.GetDistance(robotController.robotState.x, robotController.robotState.y) / PotencialDriver.maxVelocity;

                    PotencialDriver.Drive(robotController, kickingPoint.x, kickingPoint.y, eta);

                }
              */
    }

}
